#include "charge_task.h"

#include <QString>
#include "application/map/area/area.h"
#include "application/map/charge/charge_manager.h"
#include "application/map/charge/charge_point.h"
#include "kernel/agv/agv_entity.h"

namespace application {
namespace task {

ChargeTask::ChargeTask(
    std::shared_ptr<vehicle::VehicleService> vehicleService,
    std::shared_ptr<MapApplicationService> mapService,
    std::shared_ptr<dispatcher::DispatcherService> dispatcherService,
    std::shared_ptr<kernel::ILogger> logger,
    const std::string& chargePoint,
    int targetBatteryLevel)
    : SubTaskBase(logger)
    , m_vehicleService(std::move(vehicleService))
    , m_mapService(std::move(mapService))
    , m_dispatcherService(std::move(dispatcherService))
    , m_chargePoint(chargePoint)
    , m_targetBatteryLevel(targetBatteryLevel)
{
}

kernel::task::TaskStage ChargeTask::init(TaskContext& ctx)
{
    if (!m_vehicleService) {
        return finishWithError(ctx, "VehicleService not available");
    }

    if (!m_mapService) {
        return finishWithError(ctx, "MapApplicationService not available");
    }

    if (!ctx.agv) {
        return finishWithError(ctx, "No AGV assigned to charge task");
    }

    if (!ctx.agv->isOnline()) {
        logDebug("AGV offline, postpone charge task");
        return kernel::task::TaskStage::Init;
    }

    if (ctx.agv->getBatteryLevel() >= m_targetBatteryLevel) {
        logInfo("AGV " + std::to_string(ctx.agv->getNumber()) +
                " battery already at target level (" +
                std::to_string(ctx.agv->getBatteryLevel()) + "%)");
        return kernel::task::TaskStage::Cleanup;
    }

    auto chargePointPtr = ensureChargePoint(ctx);
    if (!chargePointPtr) {
        return finishWithError(ctx, "No available charge point");
    }

    if (!m_strategyInitialized) {
        initializeStrategy(ctx, chargePointPtr);
    }

    if (m_strategy == ChargeStrategy::Seer) {
        return runSeerStrategy(ctx);
    }

    return runMklmStrategy(ctx);
}

kernel::task::TaskStage ChargeTask::run(TaskContext& ctx)
{
    if (!ctx.agv) {
        return finishWithError(ctx, "AGV lost during charging");
    }

    if (!m_chargingStarted && ctx.agv->isCharging()) {
        m_chargingStarted = true;
        m_chargeStartTime = std::chrono::system_clock::now();
    }

    if (ctx.agv->isCharging()) {
        logInfo("AGV " + std::to_string(ctx.agv->getNumber()) +
                " entered charging state");
        return kernel::task::TaskStage::Cleanup;
    }

    if (isChargeTimeout()) {
        enterTimeoutState(ctx, "充电任务超时");
        return kernel::task::TaskStage::Cleanup;
    }

    logDebug("AGV not in charging state yet, waiting for charger handshake");

    return kernel::task::TaskStage::Running;
}

kernel::task::TaskStage ChargeTask::cleanup(TaskContext& ctx)
{
    logDebug("Charge task cleanup");

    if (m_strategy == ChargeStrategy::Seer && m_seerDoorClosed && ctx.agv) {
        sendSeerDockCommand(ctx, true);
        m_seerDoorClosed = false;
    }

    if (m_chargingStarted && ctx.agv) {
        logInfo("AGV " + std::to_string(ctx.agv->getNumber()) +
                " is charging at point " + m_activeChargePoint +
                ", task completed.");
    }

    m_chargingStarted = false;
    m_activeChargePoint.clear();
    m_stationPoint.clear();
    m_actionPoint.clear();
    m_navigationTarget.clear();
    m_navigationFloor = -1;
    m_strategyInitialized = false;
    m_seerPhase = SeerPhase::MoveToStation;
    return kernel::task::TaskStage::Finished;
}

bool ChargeTask::isChargeTimeout() const
{
    if (!m_chargingStarted) {
        return false;
    }

    auto now = std::chrono::system_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(
        now - m_chargeStartTime
    );

    return elapsed.count() >= CHARGE_TIMEOUT_SECONDS;
}

void ChargeTask::enterTimeoutState(TaskContext& ctx, const std::string& message)
{
    ctx.setError(message);
    ctx.requestStatus(kernel::task::TaskChainStatus::Timeout);
    logWarning(message);
}

std::shared_ptr<charge::ChargePoint> ChargeTask::ensureChargePoint(TaskContext& ctx)
{
    if (auto existing = ctx.chargePoint.lock()) {
        return existing;
    }

    std::shared_ptr<charge::ChargePoint> point;
    const int agvNumber = ctx.agv ? ctx.agv->getNumber() : -1;
    const bool hasAgv = ctx.agv != nullptr;
    const kernel::agv::AgvType agvType = hasAgv ? ctx.agv->getType() : kernel::agv::AgvType::MKLM;
    if (!m_chargePoint.empty()) {
        if (!m_mapService) {
            return nullptr;
        }
        const auto floors = m_mapService->getLoadedFloors();
        for (int floor : floors) {
            if (floor <= 0) {
                continue;
            }
            auto manager = m_mapService->getChargeManager(floor);
            if (!manager) {
                continue;
            }
            const auto pointId = QString::fromStdString(m_chargePoint);
            if (!manager->hasChargePoint(pointId)) {
                continue;
            }
            auto candidate = manager->getChargePoint(pointId);
            const bool occupiedBySelf = candidate && agvNumber != -1 &&
                candidate->isOccupied() && candidate->getCurrentOccupyingAgvId() == agvNumber;
            const bool preOccupiedBySelf = candidate && agvNumber != -1 &&
                candidate->isPreOccupied() && candidate->getPreOccupyingAgvId() == agvNumber;
            const bool available =
                candidate &&
                (!candidate->isOccupied() && !candidate->isPreOccupied()) // empty
                || occupiedBySelf // already at the charger
                || preOccupiedBySelf; // already pre-occupied by this AGV

            if (available &&
                (!hasAgv || candidate->supportsAgvType(agvType)) &&
                (agvNumber == -1 || candidate->isVehicleAllowed(agvNumber))) {
                point = candidate;
                break;
            }
        }
        if (!point) {
            return nullptr;
        }
    } else {
        if (!m_dispatcherService) {
            return nullptr;
        }
        point = m_dispatcherService->allocateChargePoint(ctx.agv, resolveCurrentFloor(ctx));
        if (!point) {
            return nullptr;
        }
        if (hasAgv && !point->supportsAgvType(agvType)) {
            return nullptr;
        }
    }

    if (ctx.agv && !point->isPreOccupied()) {
        if (!point->preOccupy(agvNumber)) {
            if (m_logger) {
                m_logger->warning(QString("Charge point %1 pre-occupy failed")
                    .arg(point->getPointId()));
            }
            return nullptr;
        }
    }

    if (auto area = point->getArea()) {
        ctx.addOccupiedArea(area);
    }

    ctx.setChargePoint(point);
    return point;
}

int ChargeTask::resolveCurrentFloor(const TaskContext& ctx) const
{
    if (!m_mapService) {
        return -1;
    }

    if (ctx.agv) {
        const auto& mapName = ctx.agv->getCurrentMap();
        if (!mapName.empty()) {
            return m_mapService->getFloorForMapName(QString::fromStdString(mapName));
        }
    }

    return m_mapService->getCurrentFloor();
}

kernel::task::TaskStage ChargeTask::runMklmStrategy(TaskContext& ctx)
{
    if (!ctx.agv) {
        return finishWithError(ctx, "No AGV assigned to charge task");
    }

    if (!ctx.agv->isCharging() &&
        ctx.agv->getCurrentStation() != m_activeChargePoint) {
        requestNavigation(ctx, m_activeChargePoint);
        return kernel::task::TaskStage::Init;
    }

    if (!m_chargingStarted) {
        bool success = m_vehicleService->startCharging(
            ctx.agv->getNumber(),
            m_activeChargePoint
        );

        if (!success) {
            return finishWithError(ctx, "Failed to start charging");
        }

        m_chargingStarted = true;
        m_chargeStartTime = std::chrono::system_clock::now();
        logInfo("Charging started successfully for AGV " +
                std::to_string(ctx.agv->getNumber()));
    }

    return kernel::task::TaskStage::Running;
}

kernel::task::TaskStage ChargeTask::runSeerStrategy(TaskContext& ctx)
{
    if (!ctx.agv) {
        return finishWithError(ctx, "No AGV assigned to charge task");
    }

    switch (m_seerPhase) {
    case SeerPhase::MoveToStation:
        if (!isAtStation(ctx, m_stationPoint)) {
            requestNavigation(ctx, m_stationPoint);
            return kernel::task::TaskStage::Init;
        }
        m_navigationTarget.clear();
        m_seerPhase = SeerPhase::CloseDo;
        [[fallthrough]];
    case SeerPhase::CloseDo:
        if (!m_seerDoorClosed) {
            if (!sendSeerDockCommand(ctx, false)) {
                return finishWithError(ctx, "Failed to send DO close command");
            }
            m_seerDoorClosed = true;
        }
        m_seerPhase = SeerPhase::MoveToAction;
        [[fallthrough]];
    case SeerPhase::MoveToAction:
        if (m_actionPoint.empty()) {
            return finishWithError(ctx, "Action point not configured for SEER charge task");
        }
        if (!isAtStation(ctx, m_actionPoint)) {
            if (!m_seerActionMoveIssued) {
                if (!sendSeerDirectMoveToAction(ctx)) {
                    return finishWithError(ctx, "Failed to send SEER direct navigation to action point");
                }
                m_seerActionMoveIssued = true;
            }
            return kernel::task::TaskStage::Running;
        }
        m_seerActionMoveIssued = false;
        m_navigationTarget.clear();
        m_seerPhase = SeerPhase::WaitCharging;
        if (!m_chargingStarted) {
            m_chargingStarted = true;
            m_chargeStartTime = std::chrono::system_clock::now();
            logInfo("SEER charging sequence reached action point, waiting for charger handshake");
        }
        break;
    case SeerPhase::WaitCharging:
        break;
    }

    return kernel::task::TaskStage::Running;
}

bool ChargeTask::isAtStation(const TaskContext& ctx, const std::string& station) const
{
    if (!ctx.agv || station.empty()) {
        return false;
    }
    return ctx.agv->getCurrentStation() == station;
}

void ChargeTask::requestNavigation(TaskContext& ctx, const std::string& station)
{
    if (station.empty()) {
        return;
    }
    m_navigationTarget = station;
    ctx.requestStatus(kernel::task::TaskChainStatus::Recalculation);
}

void ChargeTask::initializeStrategy(const TaskContext& ctx, const std::shared_ptr<charge::ChargePoint>& point)
{
    m_strategyInitialized = true;
    m_stationPoint = point->getStationPoint().toStdString();
    m_actionPoint = point->getActionPoint().toStdString();
    m_navigationFloor = point->getFloor();
    m_navigationTarget.clear();
    m_seerDoorClosed = false;
    m_seerPhase = SeerPhase::MoveToStation;
    m_seerActionMoveIssued = false;

    const bool isSeer = ctx.agv && ctx.agv->getType() == kernel::agv::AgvType::SEER;
    if (isSeer && !m_actionPoint.empty()) {
        m_strategy = ChargeStrategy::Seer;
        m_activeChargePoint = m_actionPoint;
    } else {
        m_strategy = ChargeStrategy::Mklm;
        m_activeChargePoint = m_stationPoint.empty() ? point->getPointId().toStdString() : m_stationPoint;
    }
}

bool ChargeTask::sendSeerDockCommand(TaskContext& ctx, bool open)
{
    if (!m_vehicleService || !ctx.agv) {
        return false;
    }
    
    // 本来这里要做 DO 控制的 但是现在有DO区域干这个事情 所以不用做了
    // auto result = m_vehicleService->controlDoor(ctx.agv->getNumber(), SEER_CHARGE_DO_CHANNEL, open);
    // if (!result.isSuccess()) {
    //     if (m_logger) {
    //         m_logger->error(QString("SEER DO command failed: %1")
    //             .arg(result.getErrorMessage()));
    //     }
    //     return false;
    // }

    if (m_logger) {
        m_logger->info(QString("SEER DO command sent: AGV=%1, channel=%2, open=%3")
            .arg(ctx.agv->getNumber())
            .arg(SEER_CHARGE_DO_CHANNEL)
            .arg(open));
    }
    return true;
}

bool ChargeTask::sendSeerDirectMoveToAction(TaskContext& ctx)
{
    if (!m_vehicleService || !ctx.agv || m_actionPoint.empty()) {
        return false;
    }

    // 对 SEER 直接下发使用自定位作为起点
    const std::string source = "SELF_POSITION";

    auto result = m_vehicleService->moveVehicleToStation(
        ctx.agv->getNumber(), source, m_actionPoint);
    if (!result.isSuccess()) {
        if (m_logger) {
            m_logger->error(QString("SEER direct move command failed: %1")
                                .arg(result.getErrorMessage()));
        }
        return false;
    }

    m_navigationTarget = m_actionPoint;
    if (m_logger) {
        m_logger->info(QString("SEER AGV%1 direct move: %2 -> %3")
                           .arg(ctx.agv->getNumber())
                           .arg(QString::fromStdString(source))
                           .arg(QString::fromStdString(m_actionPoint)));
    }
    return true;
}

} // namespace task
} // namespace application
